#include "All_Init.h"
#include "icm20602.h"
#include "stm32f4xx.h"                  // Device header
#include "RUI_CAN.h"
#include "WHW_Motor.h"
#include "FFC.h"
#include "tim.h"
#include "QuaternionEKF.h"
#include "operate.h"

void Everying_Init(void)
{	
	DWT_Init(168);
    CAN_Filter_Init();
    WHW_MOTER_Init();
	Matrixs_Init();
    icm20602_init();
	IMU_QuaternionEKF_Init(10, 0.001, 10000000, 1, 0.001f,0);//ekf初始化，采样与计算周期0.001s，必须与实际情况一致
	Q_info_Target.q0 = 1; Q_info_Target.q1 = 0; Q_info_Target.q2 = 0; Q_info_Target.q3 = 0;
	Q_info_Error.q0 = 1; Q_info_Error.q1 = 0; Q_info_Error.q2 = 0; Q_info_Error.q3 = 0;
	while(ICM20602_GyroOffSet());
	LED_ON;
	HAL_TIM_Base_Start(&htim4);
	HAL_TIM_Base_Start_IT(&htim2);
	HAL_TIM_Base_Start_IT(&htim5);
}
